#include "robothansplugin.h"
#include <iostream>


RobotHansPlugin::RobotHansPlugin(QObject *parent) :
    QGenericPlugin(parent)
{
}

///
/// \brief RobotHansPlugin::create
/// \param name
/// \param spec
/// \return
///
QObject* RobotHansPlugin::create(const QString& name, const QString &spec)
{
    Q_UNUSED(name);
    Q_UNUSED(spec);

    return nullptr;
}



///
/// \brief RobotHansPlugin::HR_ReadCurFSM
/// \param fsm：状态机状态
/// \return
///
int RobotHansPlugin::HR_ReadCurFSM(int &fsm)
{
    std::string strfsm;
    //返回读取fsm的状态
    return HRIF_ReadCurFSM(this->BoxId,this->RobotId,fsm,strfsm);
}

///
/// \brief judge_fsm状态判断，状态是否符合其中一个状态
/// \param fsm：输入状态
/// \param fsms：状态组
/// \return true和其中一个状态符合  false：都不符合
///
bool RobotHansPlugin::judge_fsm( int fsm, vector<int> fsms )
{
    //给予范围的for循环,modern C++
    for( auto fsmt : fsms )
    {
        if( fsmt == fsm )
            return true;
    }
    return false;
}

///
/// \brief RobotHansPlugin::HR_ReadActJointPos
/// \param jointpos
/// \return
/// 读取关节坐标
int RobotHansPlugin::HR_ReadActJointPos( std::vector<double> & jointpos)
{
    return HRIF_ReadActJointPos(this->BoxId,
                                this->RobotId,
                                jointpos[0],
                                jointpos[1],
                                jointpos[2],
                                jointpos[3],
                                jointpos[4],
                                jointpos[5]
                        );
}

///
/// \brief RobotHansPlugin::robot_set_override
/// \param override
/// \return
/// 设置机器人运动速度比例控制
int RobotHansPlugin::robot_set_override( double override  )
{
    //最大值1.0
    if( override > 1.0 )
    {
        std::cout << R"(输入速度比率过大=)" << override << std::endl;
        return _ERROR;
    }
    return HRIF_SetOverride(this->BoxId,this->RobotId,override);
}


///
/// \brief wait_state
/// \param Fsm：当状态等于输入状态时wait
/// \return
///
int RobotHansPlugin::wait_state( int Fsm )
{
    int curFsm = Fsm ;
    //等待状态离开状态
    while( curFsm == Fsm )
    {
        if( 0 !=  HR_ReadCurFSM(curFsm))
            return _ERROR;
        //要等待给别的程序执行时间
        usleep(50*1000); //50ms
    }
    return _SUCCESS;
}


///
/// \brief wait_not_state
/// \param Fsm：当状态不等于输入状态时wait
/// \return
///
int RobotHansPlugin::wait_not_state( int Fsm )
{
    int curFsm = Fsm -1 ;
    //等待状态
    while( curFsm != Fsm )
    {
        if( 0 !=  HR_ReadCurFSM(curFsm))
            return _ERROR;
        //要等待给别的程序执行时间
        usleep(50*1000); //50ms
    }
    return _SUCCESS;
}


///
/// \brief RobotHansPlugin::HR_Electrify
/// \param BoxId
/// \param RobotId
/// \return
///
int RobotHansPlugin::HR_Electrify( void )
{
    vector<int> state({RobotEnabling,Disable,Moving,LongJogMoving,RobotStopping,
        RobotDisabling,RobotOpeningFreeDriver,RobotClosingFreeDriver,FreeDriver,
        RobotHolding,StandBy,ScriptRunning,ScriptHoldHandling,ScriptHolding,ScriptStopping,ScriptStopped});
    int curFSM;
    //读取机器人状态机
    if( 0 != HR_ReadCurFSM(curFSM) )
        return _ERROR;
    //状态机判断是否已经给本体上电
    if( judge_fsm(curFSM,state) )
        return _SUCCESS;
    int nRet = HRIF_Electrify(this->BoxId);
    if (nRet != 0)
    {
        return _ERROR;
    }
    return _SUCCESS;
}


///
/// \brief robot_enable
/// \return 0 成功 -1失败
///
int RobotHansPlugin::robot_enable( void )
{
    int CurFsm ;
    int nRet;
    while( 1 )
    {
        //更新Fsm
        nRet = HR_ReadCurFSM(CurFsm) ;
        if( nRet != _SUCCESS )
        {
            ErrorCode = nRet;
            this->ErrorStr = R"(读取状态机失败)";
            return _ERROR;
        }
        //状态机等于7的时候,机器人处于未上电的状态，需要给机器人进行上电
        if( CurFsm == 7 )
        {
            nRet = HRIF_Electrify(this->BoxId);
        }
        //状态机等于2的时候,机器人处于未连接控制箱的状态，所以要先连接电控箱
        if( CurFsm == 14 )
        {
            nRet = HRIF_Connect2Controller(this->BoxId);
        }
        //状态机等于24的时候,机器人处于去使能状态，需要上使能
        if( CurFsm == 24 )
        {
            nRet = HRIF_GrpEnable(this->BoxId,this->RobotId);
        }
        //结束循环
        if( CurFsm == 33 )
        {
            break;
        }
        usleep(5000);
    }
    return _SUCCESS;
}


///
/// \brief RobotHansPlugin::HR_MoveL
/// \param pos
/// \param ref_jointpos
/// \param Vel
/// \return
///
int RobotHansPlugin::HR_MoveL( ROBOTPOS pos , std::vector<double> ref_jointpos ,double Vel)
{
    double Acc = 2500;  //加速度
    double r = 1;       //运动半径
    int Iseek = 0;      //是否开始探寻
    int IOBit = 0;
    int IOState = 0;
    string ID = "0";
    int nRet = HRIF_MoveL(this->BoxId,this->RobotId,
                          static_cast<double>(pos.x),
                          static_cast<double>(pos.y),
                          static_cast<double>(pos.z),
                          static_cast<double>(pos.rx),
                          static_cast<double>(pos.ry),
                          static_cast<double>(pos.rz),
                          ref_jointpos[0], ref_jointpos[1],
                          ref_jointpos[2], ref_jointpos[3],
                          ref_jointpos[4], ref_jointpos[5],
                          this->TcpName, coor_name, Vel, Acc, r, Iseek, IOBit, IOState, ID);
    return nRet;
}

int RobotHansPlugin::HR_MoveLByWayPoint( ROBOTPOS pos , std::vector<double> ref_jointpos ,double Vel)
{
    double Acc = 2500;  //加速度
    double r = 0;       //运动半径
    int Iseek = 0;      //是否开始探寻
    int IOBit = 0;
    int IOState = 0;
    string ID = "0";
    int nRet = HRIF_WayPoint(this->BoxId,this->RobotId,
                             1,//直线运动
                          static_cast<double>(pos.x),
                          static_cast<double>(pos.y),
                          static_cast<double>(pos.z),
                          static_cast<double>(pos.rx),
                          static_cast<double>(pos.ry),
                          static_cast<double>(pos.rz),
                          ref_jointpos[0], ref_jointpos[1],
                          ref_jointpos[2], ref_jointpos[3],
                          ref_jointpos[4], ref_jointpos[5],
                          this->TcpName, coor_name,
                           Vel, Acc, r,
                           0,
                           Iseek, IOBit, IOState, ID);
    return nRet;
}

///
/// \brief RobotHansPlugin::init：和机器人建立连接
/// \param ip
/// \param port
/// \param tcp
/// \param BoxId
/// \param RobotId
/// \return
/// 建立通信连接
bool RobotHansPlugin::Init(std::string Ip , uint16_t Port ,std::string Tcp, uint32_t BoxId  ,uint32_t RobotId)
{
    this->Ip = Ip;
    this->Port = Port;
    this->TcpName = Tcp;
    this->BoxId = BoxId;
    this->RobotId = RobotId;
    //建立和机器人通信的连接
    if( 0 != HRIF_Connect(this->BoxId,this->Ip.c_str(),this->Port )  )
    {
        return false;
    }
    //设置已经初始化
    IsInitSuccess = true;
    return true;
}

///
/// \brief RobotHansPlugin::ReadScanState：获取打磨扫描状态
/// \param State ：状态
/// \return
/// 大族DO0是我们获取的扫描状态
bool RobotHansPlugin::ReadScanState(bool &State)
{
    int StateTemp;
    //初始化
    if( !IsInitSuccess )
    {
        this->ErrorStr = R"(未初始化，请调用init函数并检查是否初始化成功)";
        return false;
    }
    if( 0 != HRIF_ReadBoxDO(this->BoxId,0,StateTemp) )
        return false;
    //把int型转为bool型
    State = static_cast<bool>(StateTemp);
    return true;
}

///
/// \brief RobotHansPlugin::ReadTakePhotoState
/// \param state
/// \return
/// 大族DO1是我们获取的拍照状态
bool RobotHansPlugin::ReadTakePhotoState(bool &State)
{
    int StateTemp;
    //初始化
    if( !IsInitSuccess )
    {
        this->ErrorStr = R"(未初始化，请调用init函数并检查是否初始化成功)";
        return false;
    }
    if( 0 != HRIF_ReadBoxDO(this->BoxId,1,StateTemp) )
        return false;
    //把int型转为bool型
    State = static_cast<bool>(StateTemp);
    return true;
}

///
/// \brief RobotHansPlugin::GetRobotState
/// \param state
/// \return
/// 获取机器人状态
bool RobotHansPlugin::GetRobotState(ROBOTSTATES &State)
{
    int curFSm ;
    //初始化
    if( !IsInitSuccess )
    {
        this->ErrorStr = R"(未初始化，请调用init函数并检查是否初始化成功)";
        return false;
    }
    if( 0 !=  HR_ReadCurFSM(curFSm))
        return false;
    vector<int> emg_state({EmergencyStopHandling,
                           EmergencyStop});
    vector<int> moving_state({Moving,
                              LongJogMoving});
    vector<int> normal_state({RobotEnabling,
                              Disable,
                              RobotStopping,
                              StandBy,
                              RobotDisabling
                             });
    vector<int> error_state({ControllerVersionError,
                             EtherCATError,
                             Error,
                             SaftyGuardError,
                             UnInitialize,
                             SaftyGuardErrorHandling,
                             RobotCollisionStop
                            });
    //急停状态
    if( judge_fsm(curFSm,emg_state) )
    {
        State = RobotStatusEmergencyStop;
    }//运动中
    else if( judge_fsm(curFSm,moving_state)  )
    {
        State = RobotStatusMoving;
    }//正常状态
    else if (judge_fsm(curFSm,normal_state))
    {
        State = RobotStatusNormal;
    }//状态错误
    else if (judge_fsm(curFSm,error_state))
    {
        State = RobotStatusError;
    }//状态未知
    else
    {
        State = RobotStatusUnknow;
    }
    return true;
}

///
/// \brief RobotHansPlugin::GetPathIndex
/// \param PathIndex
/// \return
/// 获取打磨下标
bool RobotHansPlugin::GetPathIndex(uint32_t &PathIndex)
{
    int index;
    //初始化
    if( !IsInitSuccess )
    {
        this->ErrorStr = R"(未初始化，请调用init函数并检查是否初始化成功)";
        return false;
    }
    double ratio = 0;
    if( HRIF_ReadTrackProcess(this->BoxId,this->RobotId,ratio,index) != 0 )
        return false;
    PathIndex = static_cast<uint32_t>(index);
    return true;
}


///
/// \brief RobotHansPlugin::GetRobotPos
/// \param pos_now
/// \return
/// 获取机器人位置，当前位置
bool RobotHansPlugin::GetRobotPos(ROBOTPOS &PosNow)
{
    double x,y,z,rx,ry,rz;
    //初始化
    if( !IsInitSuccess )
    {
        this->ErrorStr = R"(未初始化，请调用init函数并检查是否初始化成功)";
        return false;
    }
    //读取机器人当前位置
    if( 0 != HRIF_ReadActTcpPos(this->BoxId,this->RobotId,x,y,z,rx,ry,rz) )
        return false;
    //赋值
    PosNow = {static_cast<double>(x),
               static_cast<double>(y),
               static_cast<double>(z),
               static_cast<double>(rx),
               static_cast<double>(ry),
               static_cast<double>(rz),{0,0,0,0}};
    return true;
}

///
/// \brief RobotHansPlugin::GetQuaternion
/// \param W
/// \param X
/// \param Y
/// \param Z
/// \return
///
bool RobotHansPlugin::GetQuaternion(double W, double X, double Y, double Z)
{
    ROBOTPOS PosNow;
    if( !GetRobotPos(PosNow) )
        return false;
    if( 0 != HRIF_RPY2Quaternion (0,0, PosNow.rx , PosNow.ry , PosNow.rz , W, X, Y, Z) )
        return false;
    return true;
}

///
/// \brief RobotHansPlugin::MoveRobotTo
/// \param TargetPos
/// \param MoveSpeed
/// \param Congestion：阻塞等待完成
/// \return
/// 移动机器人到位置
bool RobotHansPlugin::MoveRobotTo(ROBOTPOS TargetPos,
                                  float MoveSpeed,
                                  bool Congestion)
{
    std::vector<double> joint_pos(6);
    //初始化
    if( !IsInitSuccess )
    {
        this->ErrorStr = R"(未初始化，请调用init函数并检查是否初始化成功)";
        return false;
    }
    //读取关节位置
    if( _SUCCESS != HR_ReadActJointPos( joint_pos ) )
    {
        this->ErrorStr = R"(读取关节位置失败)";
        return false;
    }
    //等开使能成功
    if(_SUCCESS != robot_enable())
    {
        this->ErrorStr = R"(开使能失败)";
        return false;
    }
    //设置全局速度
    if( _SUCCESS != robot_set_override(1.0))
    {
        this->ErrorStr = R"(设置全局速度比率失败)";
        return false;
    }
    //移动
    if( _SUCCESS != HR_MoveLByWayPoint(TargetPos,
                             joint_pos,
                             static_cast<double>(MoveSpeed)
                             ) )
    {
        this->ErrorStr = R"(移动机器人到位置失败)";
        return false;
    }
    bool IsDone;
    //阻塞则等待移动完成
    while(Congestion)
    {
        int nRet = HRIF_IsBlendingDone(this->BoxId,this->RobotId,IsDone);
        if( nRet != _SUCCESS )
        {
            this->ErrorStr = R"(获取路点运动情况失败)";
            return false;
        }
        else {
            if( IsDone )
                break;
        }
    }
    return true;
}


///
/// \brief RobotHansPlugin::RobotMovePath
/// \param path
/// \param speed
/// \param CntR：平滑过度参数
/// \param Congestion：阻塞等待完成
/// \return
//控制机器人按路径移动
bool RobotHansPlugin::RobotMovePath(std::vector<ROBOTPOS> Path,
                                    float MoveSpeed ,
                                    double SpeedRatio,
                                    double CntR,
                                    bool Congestion)
{
    //
    int nRet;
    if( Path.size() == 0 )
    {
        this->ErrorStr = R"(路径为空)";
        return false;
    }
    std::string PathName = "grinding_path";
    //初始化
    if( !IsInitSuccess )
    {
        this->ErrorStr = R"(未初始化，请调用init函数并检查是否初始化成功)";
        return false;
    }
    //等开使能成功
    nRet = robot_enable();
    if( nRet != _SUCCESS )
    {
        //获取错误码
        ErrorCode = nRet;
        this->ErrorStr = R"(使能失败)";
        return false;
    }
    //初始化其它参数
    nRet = HRIF_StartPushMovePathJ(this->BoxId,
                                   this->RobotId,
                                   PathName,
                                   SpeedRatio,
                                   CntR);
    if( nRet != _SUCCESS )
    {
        this->ErrorStr = R"(HRIF_StartPushMovePathJ：初始化参数失败)";
        //获取错误码
        ErrorCode = nRet;
        return false;
    }
    //轨迹参数初始化
    nRet = HRIF_InitMovePathL(
                                this->BoxId,
                                this->RobotId,
                                PathName,
                                static_cast<double>(MoveSpeed),
                                2500.0,
                                100.0,
                                coor_name,
                                this->TcpName
                                );
    if( nRet != _SUCCESS )
    {
        this->ErrorStr = R"(HRIF_InitMovePathL：初始化路径参数失败)";
        //获取错误码
        ErrorCode = nRet;
        return false;
    }
    //下发轨迹
    //引用的优势在于少了拷贝赋值的过程
    for( auto &pos : Path )
    {
        if( _SUCCESS != HRIF_PushMovePathL(this->BoxId,
                                    this->RobotId,
                                    PathName,
                                    static_cast<double>(pos.x),
                                    static_cast<double>(pos.y),
                                    static_cast<double>(pos.z),
                                    static_cast<double>(pos.rx),
                                    static_cast<double>(pos.ry),
                                    static_cast<double>(pos.rz)
                                    ))
        {
            this->ErrorStr = R"(HRIF_PushMovePathL：路径推送失败)";
            return false;
        }

    }
    //结束路径推送
    nRet = HRIF_EndPushMovePath(this->BoxId,this->RobotId,PathName);
    if( _SUCCESS != nRet )
    {
        this->ErrorStr = R"(结束路径推送失败)";
        //获取错误码
        ErrorCode = nRet;
        return false;
    } 
    //设置全局速度
    nRet = robot_set_override(1.0);
    if( _SUCCESS != nRet )
    {
        this->ErrorStr = R"(速度设置失败)";
        //获取错误码
        ErrorCode = nRet;
        return false;
    }
    //机器人移动到第一点
    if(!MoveRobotTo(Path[0],MoveSpeed))
    {
        this->ErrorStr = R"(机器人运动到第一点失败)";
        return false;
    }
    //机器人执行移动命令
    while( 1 )
    {
        nRet = HRIF_MovePathL(this->BoxId,this->RobotId,PathName);
        //未计算完成
        if(  nRet == 20018 || nRet == 20041 )
        {
            usleep(5000);
        }
        else if( nRet != _SUCCESS )
        {
            this->ErrorStr = R"(HRIF_MovePathL：移动失败)";
            //获取错误码
            ErrorCode = nRet;
            return false;
        }
        else {
            break;
        }
    }
    bool IsMoveDone = false;
    while(!IsMoveDone && Congestion)
    {
        nRet = HRIF_IsMotionDone(this->BoxId,this->RobotId,IsMoveDone);
        if( _SUCCESS != nRet )
        {
            this->ErrorStr = R"(获取运动状态失败)";
            //获取错误码
            ErrorCode = nRet;
            return false;
        }
        usleep(5000);
    }
    return true;
}

///
/// \brief RobotHansPlugin::set_tcp_name
/// \param tcp_name
/// \return
///
bool RobotHansPlugin::SetTcpName(std::string TcpName)
{
    this->TcpName = TcpName;
    return  true;
}


///
/// \brief RobotHansPlugin::GetLastError
/// \return 获取失败原因
///
std::string RobotHansPlugin::GetLastError( void )
{
    return  this->ErrorStr;
}

///
/// \brief RobotHansPlugin::ClearError
/// 清除错误消息
void RobotHansPlugin::ClearError()
{
    this->ErrorStr.clear();
}

int RobotHansPlugin::GetErrorCode()
{
    return this->ErrorCode;
}


#if QT_VERSION < 0x050000
Q_EXPORT_PLUGIN2(Robot_hans, RobotHansPlugin)
#endif // QT_VERSION < 0x050000
